#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>

#include "vmaxheap.h"
// Based on Ed's Java MaxHeap2 implementation
//                 0
//         1               2
//      3     4        5       6
//     7 8   9 10    11 12   13 14
//
// Children of node i:  2*i+1, 2*i+2
// Parent of node i: (i-1) / 2

#define MIN_SIZE 16

uint32_t vmaxheap_get_size(vmaxheap_t * vh)
{
    return vh->sz;
}

void swap(vmaxheap_t * vh, uint32_t idxa, uint32_t idxb)
{
    struct vheap_object tmp = vh->objs[idxa];
    vh->objs[idxa] = vh->objs[idxb];
    vh->objs[idxb] = tmp;
}

void vmaxheap_add(vmaxheap_t * vh, const double value, void * data)
{

    if (vh->alloc_sz == vh->sz) {
        vh->alloc_sz *= 2;
        vh->objs = (struct vheap_object *) realloc(vh->objs, sizeof(struct vheap_object)*vh->alloc_sz);
    }

    uint32_t idx = vh->sz;

    vh->objs[idx].value = value;
    vh->objs[idx].data  = data;
    vh->sz++;

    double this_score = value;
    while (idx > 0) {
        uint32_t parent = (idx-1)/2;
        if (vh->objs[parent].value >= this_score)
            break;

        swap(vh, idx, parent);
        idx = parent;
    }

}

void * vmaxheap_remove_max(vmaxheap_t * vh)
{
    void * max_data = vh->objs[0].data; // Max value
    vh->sz--;

    // move last node to top. But this may violate the heap
    // property. So recurse down to fix the heap property.
    if (vh->sz > 0) {
        vh->objs[0]  = vh->objs[vh->sz];

        uint32_t parent = 0;
        double parent_score = vh->objs[parent].value;

        while(parent < vh->sz) {

            uint32_t left = 2*parent + 1;
            uint32_t right = 2*parent + 2;

            // Compare to parent. Default to true when element is missing
            uint8_t left_smaller = (left >= vh->sz || parent_score >= vh->objs[left].value);
            uint8_t right_smaller = (right >= vh->sz || parent_score >= vh->objs[right].value);

            if (left_smaller && right_smaller)
                break; // Ordering is correct

            // Is left bigger than right?
            uint8_t left_bigger_right = (left < vh->sz && vh->objs[left].value > vh->objs[right].value);

            // Ordering is wrong.
            if (left_bigger_right) {
                swap(vh,parent,left);
                parent = left;
            } else {
                swap(vh,parent,right);
                parent = right;
            }

        }
    }

    return max_data;
}

vmaxheap_t * vmaxheap_init(uint32_t sz)
{
    vmaxheap_t * vh = (vmaxheap_t*)malloc(sizeof(vmaxheap_t));
    vh->sz = 0;

    vh->alloc_sz = MIN_SIZE;
    if (MIN_SIZE < sz)
        vh->alloc_sz = sz;

    vh->objs = (struct vheap_object*)malloc(sizeof(struct vheap_object)*vh->alloc_sz);

    vh->get_size = vmaxheap_get_size;
    vh->add = vmaxheap_add;
    vh->remove_max = vmaxheap_remove_max;

    return vh;
}

// Shallow destructor (doesn't deallocate any of the void pointers
void vmaxheap_destroy(vmaxheap_t * vh)
{
    free(vh->objs);
    free(vh);
}
